#!/usr/bin/env python
# -*- coding: utf-8 -*-

import io
import os
import time
import numpy as np


from mao_vel_commander import VelCommander


import json5
from utils import constrain_rad


file_pwd = os.path.dirname(os.path.abspath(__file__))
time_prefix = time.strftime("%Y-%m-%d_%H-%M-%S")




class TaskMao:
    def __init__(self,px4_control):
        # 实例化px4
        self.px4_control = px4_control
        # 实例化订阅和推送关系
        self.vel_commander = VelCommander()
        

    def run(self):
        px4_control = self.px4_control

        vel_commander = self.vel_commander
        # 发送trigger
        vel_commander.init()
        # px4_control.mavros_pub_th.terminate()
        # while True:
        #     vel_cmd = vel_commander.vel_cmd
        #     px4_control.moveByVelocityYawrateENU(vel_cmd[0],vel_cmd[1],vel_cmd[2])
            # time.sleep(0.1)




    
